#include "GYRO.h"
#include "dataconvert.h"
#include "delay.h"

void GYRO_Reset_Module(void);
void GYRO_Set_Angle(float angle);
void GYRO_Set_Position(int32_t pos_x, int32_t pos_y);

void GYRO_Init()
{
	GYRO_Reset_Module();
	GYRO_Set_Angle(0);
	GYRO_Set_Position(0, 0);
	delay_ms(500);
	GYRO_Set_Angle(0);
	GYRO_Set_Position(0, 0);
}

void GYRO_Reset_Module(void)	//复位CAN
{
    CanTxMsg TxMessage;
    TxMessage.StdId = POS_CID;			//控制器 ID
    TxMessage.DLC = 2;					//必须为 2
    TxMessage.IDE = CAN_Id_Standard;
    TxMessage.RTR = CAN_RTR_Data;
    TxMessage.Data[0] = 0x55;
    TxMessage.Data[1] = 0xff;
    CAN_Transmit(CAN2, &TxMessage);
}

void GYRO_Set_Angle(float angle)
{
    uData32TypeDef temp;
    CanTxMsg TxMessage;
    temp.float_form = angle;
    TxMessage.StdId = POS_CID;				//控制器 ID
    TxMessage.DLC = 4;						//必须为 4
    TxMessage.IDE = CAN_Id_Standard;
    TxMessage.RTR = CAN_RTR_Data;
    TxMessage.Data[0] = temp.char_form[0];
    TxMessage.Data[1] = temp.char_form[1];
    TxMessage.Data[2] = temp.char_form[2];
    TxMessage.Data[3] = temp.char_form[3];
    CAN_Transmit(CAN2, &TxMessage);
}

void GYRO_Set_Position(int32_t pos_x, int32_t pos_y)
{
	CanTxMsg TxMessage;
	int32_t xx = 0, yy = 0;
	xx = -(int32_t)(pos_x* 12.8318);								//car_x 即为要发送的 x 坐标
	yy = -(int32_t)(pos_y* 12.5318);								//car_y 即为要发送的 y 坐标
	TxMessage.StdId = POS_CID;				//控制器 ID
	TxMessage.DLC = 8;//必须为 8
	TxMessage.IDE = CAN_Id_Standard;
	TxMessage.RTR = CAN_RTR_Data;
	TxMessage.Data[0] = (u8)(xx >> 0);
	TxMessage.Data[1] = (u8)(xx >> 8);
	TxMessage.Data[2] = (u8)(xx >> 16);
	TxMessage.Data[3] = (u8)(xx >> 24);
	TxMessage.Data[4] = (u8)(yy >> 0);
	TxMessage.Data[5] = (u8)(yy >> 8);
	TxMessage.Data[6] = (u8)(yy >> 16);
	TxMessage.Data[7] = (u8)(yy >> 24);
	CAN_Transmit(CAN2, &TxMessage);
}
